from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range
import rclpy
from rclpy.node import Node
import math
import time
import message_filters
from tf2_ros import TransformBroadcaster
import VL53L0X    
import RPi.GPIO as GPIO
import tf_transformations
from turtlesim.msg import Pose


class FramePublisher(Node):

    def __init__(self):
        super().__init__('cranebot_frame_publisher')

        self.imu_sub = message_filters.Subscriber(self, Imu, '/cranebot/imu')
        self.tof1_sub = message_filters.Subscriber(self, Range, '/cranebot/tof1')
        self.tof2_sub = message_filters.Subscriber(self, Range, '/cranebot/tof2')
        ts = message_filters.TimeSynchronizer([self.imu_sub, self.tof1_sub, self.tof2_sub], 10)
        ts.registerCallback(self.handle_cranebot_pose)
        

    def handle_cranebot_pose(self, imu, tof1, tof2):
        self.get_logger().info('imu'+str(imu.angular_velocity.y)+'tof1'+str(tof1.range)+'tof2'+str(tof2.range))

def main():
    rclpy.init()
    node = FramePublisher()
    rclpy.spin(node)

    rclpy.shutdown()
    